import Vec3 from "./Vec3";

export default class Quaternion {
    w: number;
    x: number;
    y: number;
    z: number;
    constructor(w = 1, x = 0, y = 0, z = 0) {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    // 四元数乘法
    multiply(other: Quaternion) {
        const w = this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z;
        const x = this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y;
        const y = this.w * other.y - this.x * other.z + this.y * other.w + this.z * other.x;
        const z = this.w * other.z + this.x * other.y - this.y * other.x + this.z * other.w;
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
        return this;
    }

    normalize() {
        const magnitude = Math.sqrt(this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z);
        this.w /= magnitude;
        this.x /= magnitude;
        this.y /= magnitude;
        this.z /= magnitude;
        return this;
    }
    // 四元数转旋转矩阵
    toRotationMatrix() {
        const w = this.w;
        const x = this.x;
        const y = this.y;
        const z = this.z;

        const ww = w * w;
        const wx = w * x;
        const wy = w * y;
        const wz = w * z;
        const xx = x * x;
        const xy = x * y;
        const xz = x * z;
        const yy = y * y;
        const yz = y * z;
        const zz = z * z;

        return [
            ww + xx - yy - zz, 2 * (xy - wz), 2 * (xz + wy), 0,
            2 * (xy + wz), ww - xx + yy - zz, 2 * (yz - wx), 0,
            2 * (xz - wy), 2 * (yz + wx), ww - xx - yy + zz, 0,
            0, 0, 0, 1
        ];
    }
    clone() {
        return new Quaternion(this.w, this.x, this.y, this.z);
    }
    // 从轴角生成四元数
    static fromAxisAngle(axis: Vec3, angle: number) {
        const halfAngle = angle / 2;
        const s = Math.sin(halfAngle);
        const w = Math.cos(halfAngle);
        const x = axis.x * s;
        const y = axis.y * s;
        const z = axis.z * s;
        return new Quaternion(w, x, y, z);
    }
}